//uplink : this code proccess the data available in the SerialPort buffer
void check_uplink(void)
{
  char queryType;
  // Check for SerialPort message
  if (SerialPort.available()) {
    queryType = SerialPort.read();
    switch (queryType) {
    case 'R': // Roll PID gains
      PIDroll.Pgain = readFloatSerialPort();
      PIDroll.Igain = readFloatSerialPort();
      PIDroll.Dgain = readFloatSerialPort();
      PIDroll.Iterm = 0;  // reset I term
      break;
    case 'P': // Pitch PID gains
      PIDpitch.Pgain = readFloatSerialPort();
      PIDpitch.Igain = readFloatSerialPort();
      PIDpitch.Dgain = readFloatSerialPort();
      PIDpitch.Iterm = 0;  // reset I term
      break;
    case 'Y': // Yaw PID gains
      PIDyaw.Pgain = readFloatSerialPort();
      PIDyaw.Igain = readFloatSerialPort();
      PIDyaw.Dgain = readFloatSerialPort();
      PIDyaw.Iterm = 0;  // reset I term
      break;
    case 'C': // Receive commands
      AP_mode = SerialPort.read();
      update_command(readFloatSerialPort(), &command_throttle);
      update_command(readFloatSerialPort(), &command_roll);
      update_command(readFloatSerialPort(), &command_pitch);
      update_command_diff(readFloatSerialPort(), &command_yaw);
      break;
    case 'G': //print gains
      SerialPort.print("KPR:");
      SerialPort.print(PIDroll.Pgain);
      SerialPort.print(",KIR:");
      SerialPort.print(PIDroll.Igain);
      SerialPort.print(",KDR:");
      SerialPort.print(PIDroll.Dgain);

      SerialPort.print("KPP:");
      SerialPort.print(PIDpitch.Pgain);
      SerialPort.print(",KIP:");
      SerialPort.print(PIDpitch.Igain);
      SerialPort.print(",KDP:");
      SerialPort.print(PIDpitch.Dgain);

      SerialPort.print("KPY:");
      SerialPort.print(PIDyaw.Pgain);
      SerialPort.print(",KIY:");
      SerialPort.print(PIDyaw.Igain);
      SerialPort.print(",KDY:");
      SerialPort.print(PIDyaw.Dgain);
      break;
    case 'O': // Take the gyro offset values
      for(int i=0;i<150;i++) //read 150 times => 3 secs
      {
        Read_adc_raw();
        for(int y=0; y<=2; y++)   // Read initial ADC values for gyros offset
          AN_OFFSET[y] = (AN_OFFSET[y]*4 + AN[y]) /5;
        delay(20);  //read at 50 Hz
      }

      /*SerialPort.print("OFFSET0:");
       SerialPort.print(AN_OFFSET[0]);
       SerialPort.print(",OFFSET1:");
       SerialPort.print(AN_OFFSET[1]);
       SerialPort.print(",OFFSET2:");
       SerialPort.print(AN_OFFSET[2]);  
       SerialPort.print(",OFFSET3:");
       SerialPort.print(AN_OFFSET[3]);
       SerialPort.print(",OFFSET4:");
       SerialPort.print(AN_OFFSET[4]);
       SerialPort.print(",OFFSET5:");
       SerialPort.print(AN_OFFSET[5]);
       SerialPort.print(",");*/
      break;
    case 'S': //Performance reporting
#if PERFORMANCE_REPORTING == 1  
      timer_tmp_old = micros();
      SerialPort.print("Gdt ");
      SerialPort.print(1000*G_Dt);
      SerialPort.print(" Gdtmax ");
      SerialPort.print(1000*G_Dt_max);//en ms
      SerialPort.print(" sat ");
      SerialPort.print(gyro_sat_count);
      /* SerialPort.print(" tdcm ");
       SerialPort.print(t_dcm/nb_dcm);
       SerialPort.print(" talgo ");
       SerialPort.print(t_read/500.);
       SerialPort.print(" tlink ");
       SerialPort.print(t_link/50.);/*
       
      /* SerialPort.print(" update ");
       SerialPort.print(tt_update/nb_dcm);
       SerialPort.print(" normalize ");
       SerialPort.print(tt_normalize/nb_dcm);
       SerialPort.print(" Drift_correction ");
       SerialPort.print(tt_Drift_correction/nb_dcm);
       SerialPort.print(" Euler_angles ");
       SerialPort.print(tt_Euler_angles/nb_dcm);*/

      //print
      /*    Omega_P[3];       //Omega Proportional correction
       Omega_I[3]  */      //Omega Integrator correction

      G_Dt_max = 0;
      gyro_sat_count = 0;
      /*  t_dcm = 0;
       nb_dcm = 0;
       t_read = 0;
       t_link = 0;*/
      /* tt_update = 0;
       tt_normalize = 0;
       tt_Drift_correction = 0;
       tt_Euler_angles = 0; */

#endif
      break;
    }
  }
}

// Used to read floating point values from the SerialPort port
// Values betweens -327 and 327 with a resolution of 0.01
float readFloatSerialPort() {
  byte c1 = SerialPort.read();
  byte c2 = SerialPort.read();
  long i = c1 + (c2 << 8);
  return (i + 32768)/100.;
  //return (signed(SerialPort.read() + (SerialPort.read() << 8)) - 32768)/100.;
}









